#ifndef ROVER_COMPUTER_
#define ROVER_COMPUTER_

#include "include/rover_computer.h"

RoverComputer::RoverComputer(const char* server_name, const char* ardui_fd, int baud, int portno){
  this->server_name = server_name;
  this->ardui_fd = ardui_fd;
  this->baud = baud;
  this->portno = portno;
}

RoverComputer::~RoverComputer(){
  delete ardui_comm;
  delete port_comm;
}

void RoverComputer::init(){
#ifndef NO_RVR_ARDUI
  printf("Attempting serial conneition.\n");
  this->ardui_comm = new SerialComm(this->ardui_fd, this->baud);
  // Sleep to make the serial port valid
  sleep(2);
  printf("Sucess on serial connection.\n");
#endif /* #ifndef NO_RVR_ARDUI */
#ifndef NO_SOCK
  printf("Atttempting socket connection.\n");
  this->port_comm = new PortComm(this->server_name,
  				 this->portno,
				 PortComm::CLIENT);
  printf("Sucess on socket connection.\n");
#endif /* #ifndef NO_SOCK */
}

void RoverComputer::run(void){
  printf("Entering Run of RoverComputer. Now init and connection attempt.\n");
  init();
  printf("Completion of init. Connection Established.\n");

  int count = 0;
  int size = Command::getStringLength();

  struct timeval lastSent, current;
  gettimeofday(&lastSent, NULL);

  Commander* commander = new Commander();

  while(true){
    unsigned char* buffer;
    int length = 0;

#ifndef NO_SOCK
    this->port_comm->read_port(&buffer, &length);

    InputState* state = InputState::getInputState(buffer);
    Command* command;
    if(state){
      command = commander->getCommand(state, NULL);
      //command = new Command(state, NULL);
    }else{
      continue;
    }

    unsigned char* commandString;
    command->toString(&commandString);
#endif /* #ifndef NO_SOCK */

    gettimeofday(&current, NULL);
    long seconds = current.tv_sec - lastSent.tv_sec;
    long useconds = current.tv_usec - lastSent.tv_usec;

    long mtime = ((seconds) * 1000 + useconds/1000.0) + 0.5;

    if(mtime > ROVER_DELAY_MS){
      count++;

      printf("%d\n", count);
#ifndef NO_RVR_ARDUI
      if(this->ardui_comm->sendString(commandString, size) == FAILURE_COMM){
	fprintf(stderr, "Unable to send string.\n");
      } 
      
      unsigned char* returnString = NULL;
      if(this->ardui_comm->readString(&returnString, '\n') != FAILURE_COMM){
	if(returnString){
	  printf("%s\n", returnString);
	}else{
	  fprintf(stderr, "Unable to get return string.\n");
	}
      }
      delete[] returnString;
#endif /* #ifndef NO_RVR_ARDUI */
      gettimeofday(&lastSent, NULL);
    }


#ifdef DEBUG
    printf("//INPUTSTATE----------------------------------\n");
    print_name_values(buffer, InputState::getStringLength(), InputStateNames::names);
    printf("//COMMAND-------------------------------------\n");
    print_name_values(commandString, Command::getStringLength(), CommandStateNames::names);
#endif /* #ifdef DEBUG */

    delete commandString;
    delete command;
    delete state;
  }
  delete commander;
}

#endif /* #ifndef ROVER_COMPUTER_ */
